import os
import yaml
import sys
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.actions import ExecuteProcess
from moveit_configs_utils import MoveItConfigsBuilder

def generate_launch_description():
    version_info = sys.version_info
    print("version.major：", version_info.major)
    print("version.minor：", version_info.minor)
    print("== 1 ==")

    moveit_config = (
        MoveItConfigsBuilder("brt805a",package_name="brtixrz0805a_moveit_config")
        .robot_description(file_path="config/brt805a.urdf.xacro")
        .trajectory_execution(file_path="config/moveit_controllers.yaml")
        .planning_pipelines(pipelines=["ompl"])
        .to_moveit_configs()
    )
    
    print("== 11 ==")
    #print( moveit_config )
    print("== 12 ==")
    # Set planning pipeline parameters
    moveit_config.planning_pipelines["ompl"]["brt_arm"][
        "enforce_constrained_state_space"
    ] = True
    moveit_config.planning_pipelines["ompl"]["brt_arm"][
        "projection_evaluator"
    ] = "joints(brt_joint2,brt_joint3)"
    # Start the actual move_group node/action server
    run_move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="both",
        parameters=[moveit_config.to_dict()],
        #arguments=['--ros-args', '--log-level', 'debug'],
    )
    
    # ros2_control using FakeSystem as hardware
    ros2_controllers_path = os.path.join(
        get_package_share_directory("brtixrz0805a_moveit_config"),
        "config",
        "ros2_controllers.yaml",
    )
    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[moveit_config.robot_description, ros2_controllers_path],
        #arguments=['--ros-args', '--log-level', 'debug'],
        output="both",
    )
    load_controllers = []
    for controller in [
        "brt_arm_controller",
        "joint_state_broadcaster",
    ]:
        load_controllers += [
            ExecuteProcess(
                cmd=["ros2 run controller_manager spawner {}".format(controller)],
                shell=True,
                output="screen",
            )
        ]
  
    demo_node = Node(
        package="moveit2_tutorials",
        executable="brt",
        output="both",
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
        ],
        #arguments=['--ros-args', '--log-level', 'debug'],
    )
	
    # RViz
    rviz_config_file = (
        get_package_share_directory("moveit2_tutorials")
        + "/launch/ompl_constrained_planning_brt.rviz"
    )
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_config_file],
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
        ],
    )

    # Static TF
    static_tf = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="static_transform_publisher",
        output="log",
        arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "brt_link0"],
    )

    # Publish TF
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="both",
        parameters=[moveit_config.robot_description],
    )

    robot_state_publisher_gui = Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
    )

    return LaunchDescription(
        [
            rviz_node,
            static_tf,
            ros2_control_node,
            robot_state_publisher,
            robot_state_publisher_gui,
            run_move_group_node,
            demo_node,
        ]
        + load_controllers
    )
